// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
//     backends/cpp/templates/function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------

#pragma once

#include <matrix/math.hpp>

namespace sym {

/**
 * This function was autogenerated from a symbolic function. Do not modify by hand.
 *
 * Symbolic function: init_wind_using_airspeed
 *
 * Args:
 *     v_local: Matrix31
 *     heading: Scalar
 *     airspeed: Scalar
 *     v_var: Scalar
 *     heading_var: Scalar
 *     sideslip_var: Scalar
 *     airspeed_var: Scalar
 *
 * Outputs:
 *     wind: Matrix21
 *     P: Matrix22
 */
template <typename Scalar>
void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Scalar heading,
                           const Scalar airspeed, const Scalar v_var, const Scalar heading_var,
                           const Scalar sideslip_var, const Scalar airspeed_var,
                           matrix::Matrix<Scalar, 2, 1>* const wind = nullptr,
                           matrix::Matrix<Scalar, 2, 2>* const P = nullptr) {
  // Total ops: 22

  // Input arrays

  // Intermediate terms (7)
  const Scalar _tmp0 = std::cos(heading);
  const Scalar _tmp1 = std::sin(heading);
  const Scalar _tmp2 = std::pow(_tmp1, Scalar(2));
  const Scalar _tmp3 = std::pow(airspeed, Scalar(2)) * sideslip_var;
  const Scalar _tmp4 = std::pow(_tmp0, Scalar(2));
  const Scalar _tmp5 = _tmp0 * _tmp1;
  const Scalar _tmp6 = -_tmp3 * _tmp5 + _tmp5 * airspeed_var;

  // Output terms (2)
  if (wind != nullptr) {
    matrix::Matrix<Scalar, 2, 1>& _wind = (*wind);

    _wind(0, 0) = -_tmp0 * airspeed + v_local(0, 0);
    _wind(1, 0) = -_tmp1 * airspeed + v_local(1, 0);
  }

  if (P != nullptr) {
    matrix::Matrix<Scalar, 2, 2>& _p = (*P);

    _p(0, 0) = _tmp2 * _tmp3 + _tmp4 * airspeed_var + v_var;
    _p(1, 0) = _tmp6;
    _p(0, 1) = _tmp6;
    _p(1, 1) = _tmp2 * airspeed_var + _tmp3 * _tmp4 + v_var;
  }
}  // NOLINT(readability/fn_size)

// NOLINTNEXTLINE(readability/fn_size)
}  // namespace sym
